#include "Matrix.hh"

#include "common/stl.h"
#include "tools/constants.hh"
#include "tools/Vector.hh"


// Ctors - Dtors
Matrix::Matrix() : _save(0)
{
	for (int i(0); i < 4; ++i)
		for (int j(0); j < 4; ++j)
			_values[i][j] = 0.f;
}


Matrix::Matrix(bool identity) : _save(0)
{
    // Initialize values to 0 if !identity
	if (!identity)
	{
		for (int i(0); i < 4; ++i)
			for (int j(0); j < 4; ++j)
				_values[i][j] = 0.f;
		return;
	}

	_values[0][0] = 1.f;	_values[0][1] = 0.f;	_values[0][2] = 0.f;	_values[0][3] = 0.f;
	_values[1][0] = 0.f;	_values[1][1] = 1.f;	_values[1][2] = 0.f;	_values[1][3] = 0.f;
	_values[2][0] = 0.f;	_values[2][1] = 0.f;	_values[2][2] = 1.f;	_values[2][3] = 0.f;
	_values[3][0] = 0.f;	_values[3][1] = 0.f;	_values[3][2] = 0.f;	_values[3][3] = 1.f;
}


Matrix::Matrix(float* val, int nb_elts) : _save(0)
{
    #ifdef DEBUG
        assert(nb_elts > 0 && nb_elts <= 16);
    #endif

    int i(0);
    // Initialize values with \param val content
    for (; i < nb_elts; ++i)
        _values[i / 4][i % 4] = val[i];

    // fill with 0
    for (; i < 16; ++i)
        _values[i / 4][i % 4] = 0.f;
}


Matrix::Matrix(Matrix const &m) : _save(0)
{
    for (int i(0); i < 4; ++i)
    {
	    for (int j(0); j < 4; ++j)
        	_values[i][j] = m._values[i][j];
    }
}


Matrix::Matrix(Vector u, Vector v, Vector w) : _save(0)
{
	_values[0][0] = u.x(); _values[0][1] = v.x(); _values[0][2] = w.x(); _values[0][3] = 0;
	_values[1][0] = u.y(); _values[1][1] = v.y(); _values[1][2] = w.y(); _values[1][3] = 0;
	_values[2][0] = u.z(); _values[2][1] = v.z(); _values[2][2] = w.z(); _values[2][3] = 0;

	// last line
	for (int j(0); j < 4; ++j)
		_values[3][j] = 0.f;
}


Matrix::~Matrix()
{
    stackClear();
}
// end Ctors - Dtor


// getter
float* Matrix::getValues()
{
    return (float*) _values;
}


void Matrix::setValues(unsigned int i, unsigned int j, float val)
{
    _values[i][j] = val;
}


// Operators overloading
Matrix Matrix::operator* (Matrix const &m)
{
    Matrix res;

	for (int i(0); i < 4; i++)
	{
		// lines * columns
		for (int j(0); j < 4; j++)
		{
			// one line * one column
			for (int k(0); k < 4; k++)
				res._values[j][i] += _values[j][k] * m._values[k][i];
		}
	}

	return res;
}

Matrix& Matrix::operator= (Matrix const &m)
{
	for (int i(0); i < 4; ++i)
	{
		for (int j(0); j < 4; ++j)
        	_values[i][j] = m._values[i][j];
	}

    return (*this);
}
// end Operators overloading


float& Matrix::get(unsigned int i, unsigned int j) // private method
{
	return _values[i][j];
}


bool Matrix::stackPush()
{
    Matrix* newMatrix = new Matrix;

    if (!newMatrix)
        return false;

    *newMatrix = *this;
    newMatrix->_save = _save;
    _save = newMatrix;

    return true;
}

bool Matrix::stackPop()
{
    Matrix* tmp = _save;

    if (!tmp)
        return false;

    _save = tmp->_save;
    *this = *tmp;
    delete tmp;

    return true;
}

void Matrix::stackClear()
{
    while (stackPop())
        ;
}


// NOTE: we could 'optimize' this (not doing a loop)
void Matrix::loadIdentity()
{
    // remove any save
    stackClear(); // really should do this ?

	_values[0][0] = 1.f;	_values[0][1] = 0.f;	_values[0][2] = 0.f;	_values[0][3] = 0.f;
	_values[1][0] = 0.f;	_values[1][1] = 1.f;	_values[1][2] = 0.f;	_values[1][3] = 0.f;
	_values[2][0] = 0.f;	_values[2][1] = 0.f;	_values[2][2] = 1.f;	_values[2][3] = 0.f;
	_values[3][0] = 0.f;	_values[3][1] = 0.f;	_values[3][2] = 0.f;	_values[3][3] = 1.f;
}


void Matrix::translate(Vector v)
{
    Matrix t; // translating Matrix

    t.loadIdentity();
    t._values[0][3] = v.getX();
    t._values[1][3] = v.getY();
    t._values[2][3] = v.getZ();

    *this = *this * t;
}


void Matrix::scale(Vector v)
{
    Matrix s; // scaling Matrix

    s._values[0][0] = v.getX();
    s._values[1][1] = v.getY();
    s._values[2][2] = v.getZ();
    s._values[3][3] = 1.0f;

    *this = *this * s;
}


void Matrix::rotate(float angle, Vector v)
{
    v.normalize();

    // converting angle in radians
    angle *= MATHS_PI_DIV_180;

    float x = v.getX();
    float y = v.getY();
    float z = v.getZ();

    float values[11];

    // rotating
    values[0] = x * x * (1.0f - cos(angle)) + cos(angle);
    values[1] = x * y * (1.0f - cos(angle)) - z * sin(angle);
    values[2] = x * z * (1.0f - cos(angle)) + y * sin(angle);
    values[3] = 0;

    values[4] = x * y * (1.0f - cos(angle)) + z * sin(angle);
    values[5] = y * y * (1.0f - cos(angle)) + cos(angle);
    values[6] = y * z * (1.0f - cos(angle)) - x * sin(angle);
    values[7] = 0;

    values[8] = x * z * (1.0f - cos(angle)) - y * sin(angle);
    values[9] = y * z * (1.0f - cos(angle)) + x * sin(angle);
    values[10] = z * z * (1.0f - cos(angle)) + cos(angle);

    Matrix rotation(values, 11);

    rotation[3][3] = 1.0f;

    *this = *this * rotation;
}


#ifdef DEBUG
    void Matrix::print()
    {
        std::cout << "<printing Matrix...>" << std::endl;
        for (int i(0); i < 4; ++i)
        {
			for (int j(0); j < 4; ++j)
			{
	        	std::cout.width(12);
				std::cout.fill(' ');
				std::cout << std::left << _values[i][j] << ' ';
			}
        	std::cout << std::endl;
        }
        std::cout << "... printed" << std::endl;
    }

	#ifdef DEBUG_MATRIX
		void Matrix::testValues()
		{
			for (int i(0); i < 16; ++i)
			{
				for (int j(0); j < 4; ++j)
					setValues(i, j, i * 4 + j);
			}

			std::cout << "testing" << std::endl;
			print();
		}


		void Matrix::testProduct()
		{
			Matrix A;
			Matrix B;

			for (int i(0); i < 4; ++i)
				for (int j(0); j < 4; ++j)
				{
					A[i][j] = i * 4 + j;
					B[i][j] = i * 4 + j;
				}

			std::cout << "A:" << std::endl;
			A.print();
			std::cout << "B:" << std::endl;
			B.print();

			Matrix C = A * B;

			std::cout << "C = A * B:" << std::endl;
			C.print();

			/* C should be
			 * 56.000  62.000  68.000  74.000
			 * 152.000 174.000 196.000 218.000
			 * 248.000 286.000 324.000 362.000
			 * 344.000 398.000 452.000 506.000
			 */
		}
	#endif // DEBUG_MATRIX
#endif
